#include "DiscPickupControl.h"
#include "../NetworkMap.h"

DiscPickupControl::DiscPickupControl() {
	Requires(CommandBase::discHandling);
	latch = false;
	oldlatch = false;
	breachUp = false;
	blatch = false;
	bolatch = false;
	inprogress = false;
	l2 = false;
	ol2 = false;
	extendPin = false;
	conveyorOn = true;
}
void DiscPickupControl::Initialize() {
	Command::SetInterruptible(false);
	Timer pinTimer;
}

void DiscPickupControl::Execute() {

	//Dump mode
	//	blatch = oi->shooterJoystick.GetRawButton(10);
		if (!discHandling->AUTO) {
			if (oi->shooterJoystick.GetRawButton(5)) {
				discHandling->ControlConveyorState(DiscHandling::kConveyorDown);
			}
		}
	//	if (blatch) {
	//		DiscHandling::RetractPin(discHandling);
	////	}
	//	if (blatch && !bolatch) { //rise
	//
	//		discHandling->ControlConveyorState(DiscHandling::kConveyorDown);
	//		discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetDown);
	//	}
	//	if (bolatch && !blatch) { //fall
	//		discHandling->ControlConveyorState(DiscHandling::kConveyorOff);
	//		discHandling->startConveyor->StartSingle(0.25);
	//		discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetOff);
	//	}
	//	bolatch = blatch;

	//	latch = discHandling->topProxSensor.Get(); //top sensor is 2
	//	if (latch && !oldlatch) {
	//		extendPin = true;
	//		discHandling->extendPin->StartSingle(0.0);
	//	}
	//	if (oldlatch && !latch) {
	//		extendPin = false;
	//	}
	//	if (!(extendPin || breach->IsBreachUp())) {
	//		discHandling->retractPin->StartSingle(0.1);
	//	}
	//	oldlatch = latch;


	if (discHandling->topProxSensor.Get() == 0 || breach->IsBreachUp()) //If one is ready to breach OR breach is up
	{
		DiscHandling::ExtendPin(discHandling);
	} else {
		DiscHandling::RetractPin(discHandling);
	}

	if (discHandling->bottomProxSensor.Get() == 0
			&& discHandling->middleProxSensor.Get() == 0
			&& discHandling->topProxSensor.Get() == 0
			&& discHandling->breachProxSensor.Get() == 0) {
		conveyorOn = false;
		DiscHandling::StopConveyor(discHandling);
	} else {
		if (!conveyorOn) {
			conveyorOn = true;
			DiscHandling::StartConveyor(discHandling);
		}
	}
	// IN button
	//	l2 = oi->shooterJoystick.GetRawButton(6);
	//	if (l2 && !ol2)
	//	{
	//		DSPrintLine(LINE3, "B6: %d", (int)l2);
	//		discHandling->ControlConveyorState(DiscHandling::kConveyorUp);
	//	}
	//	if (ol2 && !l2)
	//	{
	//		discHandling->ControlConveyorState(DiscHandling::kConveyorOff);
	//	}
	//	ol2 = l2;

	if (discHandling->GetDiscNumber() == 0) {
		shooter->shooterVictor.SetRaw(255);
	} else {
		shooter->shooterVictor.SetRaw(255);
	}

	//	latch = oi->shooterJoystick.GetRawButton(1); //get trigger
	//	if (latch && !oldlatch) //Rising edge
	//	{
	//		if (discHandling->breachProxSensor.Get() == 0) {
	//			//If we have one in the shooter
	//			shooter->Pulse();
	//			return;
	//		}
	//		if (discHandling->topProxSensor.Get() == 0) {
	//			//If we have one ready to breach
	//			Breach::Pulse(breach);
	//			return;
	//		}
	//		//discHandling->ControlConveyorState(DiscHandling::kConveyorUp);
	//	}

	//	if (oldlatch && !latch) //Falling edge
	//	{
	//		discHandling->ControlConveyorState(DiscHandling::kConveyorOff);
	//	}
	//	oldlatch = latch;
	CommandBase::client->SetNWFloat(shtVic, shooter->shooterVictor.Get());

	//	if (oi->driverJoystick.GetRawButton(7)) {
	//		discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetOff);
	//	} else if (discHandling->GetDiscNumber() == 4) {
	//		discHandling->ControlConveyorState(discHandling->kConveyorOff);
	//		discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetUp);
	//	} else if (breach->IsBreachUp()) {
	//		discHandling->ControlConveyorState(discHandling->kConveyorOff);
	//		discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetDown);
	//	} else if (discHandling->topProxSensor.Get() == 0
	//			&& discHandling->middleProxSensor.Get() == 0) {
	//		discHandling->ControlConveyorState(discHandling->kConveyorOff);
	//		discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetDown);
	//	} else {
	//		discHandling->ControlConveyorState(discHandling->kConveyorUp);
	//		discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetDown);
	//	}

}
bool DiscPickupControl::IsFinished() {
	return false;
}
void DiscPickupControl::End() {
	discHandling->ControlConveyorState(discHandling->kConveyorDown);
	discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetUp);
}
void DiscPickupControl::Interrupted() {
	discHandling->ControlConveyorState(discHandling->kConveyorDown);
	discMagnet->DiscMagnetControl(discMagnet->kDiscMagnetUp);
}
